/*****************************************************
** Author: 
** Date: 2021-05-01
*
*****************************************************/

#define SCP_MAJOR_VERSION       0x01
#define SCP_MINOR_VERSION       0x00

#define SCP_MAGIC       0xdeadc0de
#define SCP_VERSION     (SCP_MAJOR_VERSION << 8 | SCP_MINOR_VERSION)

#define SIZEOF_MSG_HEADER       sizeof(ScpHeaderStru)

enum {
    TYPE_RESV,
    TYPE_MGMT,
    TYPE_CTRL,
    TYPE_DATA,
};

enum {
    TAG_CAR_GEAR = 1,
    TAG_CAR_GEAR_WEIGHT = 2,
    TAG_CAR_GEAR_STEP = 3,
    TAG_CAR_DIRECTION = 4,
    TAG_CAR_DIRECTION_ANGLE = 5,
    TAG_CAR_DIRECTION_STEP = 6,
};

enum {
    VALUE_CAR_GEAR_P,
    VALUE_CAR_GEAR_D,
    VALUE_CAR_GEAR_R,
};

enum {
    VALUE_CAR_DIRECTION_LEFT,
    VALUE_CAR_DIRECTION_RIGHT,
};

typedef struct Tlv {
    unsigned short tag;
    unsigned short length;
    unsigned char value[0];
} TlvStru;

typedef struct ScpHeader {
    unsigned int magic;
    unsigned short version;
    unsigned short payloadSize;
    unsigned short type;
    unsigned short src;
    unsigned short dst;
    unsigned short crc;
} ScpHeaderStru;

/**
 * @brief
 * @param  None
 * @retval None
 */
int CreateMsgHeader(unsigned short ctrltype, unsigned short bodysize, 
                    unsigned char *output, int maxsize)
{
    ScpHeaderStru *header = (ScpHeaderStru *)output;
    TlvStru *tlv;

    header->magic = SCP_MAGIC;
    header->version = SCP_VERSION;
    header->type = ctrltype;
    header->src = 0x00;
    header->dst = 0x00;
    header->crc = 0x00;
    header->payloadSize = bodysize;
}

int CreateCtrlMsg(int ctrl, unsigned char *output, int maxsize)
{
    int bodysize = 0;

    switch (ctrl) {
        case CTRL_CAR_FORWARD:
        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_GEAR;
        tlv->length = 1;
        tlv->value[0] = VALUE_CAR_GEAR_D;
        bodysize += sizeof(TlvStru) + 1;

        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_GEAR_WEIGHT;
        tlv->length = 1;
        tlv->value[0] = 90;
        bodysize += sizeof(TlvStru) + 1;
        break;

        case CTRL_CAR_BACKWARD:
        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_GEAR;
        tlv->length = 1;
        tlv->value[0] = VALUE_CAR_GEAR_R;
        bodysize += sizeof(TlvStru) + 1;

        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_GEAR_WEIGHT;
        tlv->length = 1;
        tlv->value[0] = 90;
        bodysize += sizeof(TlvStru) + 1;
        break;

        case CTRL_CAR_LEFT:
        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_DIRECTION;
        tlv->length = 1;
        tlv->value[0] = VALUE_CAR_DIRECTION_LEFT;
        bodysize += sizeof(TlvStru) + 1;

        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_DIRECTION_ANGLE;
        tlv->length = 1;
        tlv->value[0] = 64;
        bodysize += sizeof(TlvStru) + 1;
        break;

        case CTRL_CAR_RIGHT:
        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_DIRECTION;
        tlv->length = 1;
        tlv->value[0] = VALUE_CAR_DIRECTION_RIGHT;
        bodysize += sizeof(TlvStru) + 1;

        tlv = (TlvStru *)(output + bodysize);
        tlv->tag = TAG_CAR_DIRECTION_ANGLE;
        tlv->length = 1;
        tlv->value[0] = 64;
        bodysize += sizeof(TlvStru) + 1;
        break;

        default:
        break;
    }

    return bodysize;
}

/* End of file */



